
function idat = fsb_realign_3D_ctm(idat,ref)

% FSB : 3D-CoM based realignment in phase direction
%
% EXAMPLE:
% idat = fsb_realign_3D_ctm(idat,1)
%
% INPUT:
% idat:         4-D image data
% ref:          Reference volume
%
% OUTPUT:
% idat:         A coarsely realigned 4-D image data volume
%
% CALLED by:
% FSB.m
% fsb_realign.m
% 
%
% NOTES:
% Very coarse realignment, indeed...
% But speed was the main goal here.
% As the algorithm averages between successive volumes, there is some
% additional smoothness introduced in the images.
% Useful for realignment of volumes that are not in need for
% rotation or shifts in other directions.
% Steffen Stoewer 28/11/07
% Center of mass code by George A Keliris 2006
%
% Copyright 2010 MPI for Biological Cybernetics
% Author: Steffen Stoewer
% License:GNU GPL, no express or implied warranties
% 
% $Revision 1.0
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
% Get basic 4D information
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
tic;
[a,b,c,d]=size(idat);%get idat parameters

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
% Determining 3D-Center-of-Mass
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
h = waitbar(0,'Determining center of mass...');

[ix iy]=find(ones(a,b)); % Gives indices of x and y

iy=repmat(iy,c,1); % Repeat the y index z times
linear_ind=find(ones(a,b,c));
ctr_mass_xyz = zeros(d,1); % Preallocate for speed

for t=1:d
    img=double(idat(:,:,:,t));
    w=img(linear_ind);
    waitbar(t/d);
    ctr_mass_xyz(t,:)=sum((w.*iy )/sum(w));
end

dctr_mass_r(:,1)=ctr_mass_xyz(:,1)-ctr_mass_xyz(ref,1);
 
close(h);

figure(110);
set(gcf, 'Name','Estimated center-of-mass shift (A-P)');
plot (dctr_mass_r,'b');
xlabel('time -> [img]');
ylabel('center-of-mass shift -> [voxel]');

disp (['Max. negative translation: ' num2str(min(dctr_mass_r)) ' voxel']);
disp (['Max. positive translation: ' num2str(max(dctr_mass_r)) ' voxel']);


%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
% Shift every single volume back or forth
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

dc = fix(dctr_mass_r);

g = waitbar(0,'Realigning...');

for x = 1:d

    p_idat = idat(:,:,:,x);

    %~~~~~~~~~~~~~~~~~~
    % Shift volumes if necessary
    %~~~~~~~~~~~~~~~~~~
    if dc(x)~=0 && ~isnan(dc(x));
        p_idat = circshift(p_idat,[0 -dc(x)]);
    end

    dc_rem = dctr_mass_r(x)-dc(x);

    %~~~~~~~~~~~~~~~~~~
    % Create shifted volumes for averaging
    %~~~~~~~~~~~~~~~~~~
    p_idat1 = circshift(p_idat,[0 1]);
    p_idat2 = circshift (p_idat,[0 -1]);

    %~~~~~~~~~~~~~~~~~~
    % Average original and shifted volume to approximate realignment
    %~~~~~~~~~~~~~~~~~~
    if dc_rem>0;
        idat(:,:,:,x) = (p_idat2*dc_rem)+(p_idat*(1-dc_rem));
    elseif dc_rem<0;
        idat(:,:,:,x) = (p_idat1*abs(dc_rem))+(p_idat*(1-abs(dc_rem)));
    end

    waitbar(x/d);
end

close(g);

toc;
end
